void copyPoints(kinectPCL::ConstPtr fromCloud, kinectPCL::ConstPtr toCloud) {
	kinectPCL::const_iterator iter = fromCloud->begin();
	printf("Copying %i points\n", (int)(fromCloud->end()-fromCloud->begin()));
	while (iter != fromCloud->end()) {
		toCloud->push_back(*iter);
		iter++;
	}
}




void PointsCallback(const kinectPCL::ConstPtr& nextCloud) {
	kinectPCL::Ptr& nextCloudPtr = (kinectPCL::Ptr&)nextCloud;
	if (fullModel->size() == 0) {
		*fullModel = *nextCloud;
	}
	else {
		pcl::IterativeClosestPoint<PointType, PointType> icp;
		icp.setInputCloud(nextCloud);
		icp.setInputTarget(fullModel);
		printf("Next Cloud:\n");
		printPoints(nextCloudPtr, 3);
		printf("Full Model:\n");
		printPoints(fullModel, 3);
		kinectPCL Final;
		icp.align(Final);
		printf("has converged: %i, score: %g\n", icp.hasConverged(), icp.getFitnessScore());
		*fullModel += Final;
	}
}

int main(int argc, char** argv) {
    ros::init(argc, argv, "testconversion");
    ros::NodeHandle n;
    
    ros::Subscriber rfidSub = n.subscribe<kinectPCL>("/camera/depth/points", 100, PointsCallback);
    ros::Rate loop_rate(5);

    while(n.ok()){
        ros::spinOnce();    
        loop_rate.sleep();
    }
    return 0;
}




/////////////////////////////////////////////////////////
		//Do ICP on a downsampled version of the scans
		kinectPCL::Ptr toAlign(new kinectPCL);
		kinectPCL::Ptr alignModel(new kinectPCL);
		DownSample(nextCloud, toAlign, COARSE_LEAF_SIZE);
		DownSample(fullModel, alignModel, COARSE_LEAF_SIZE);
		pcl::IterativeClosestPoint<PointType, PointType> icp;
		//Make the initial guess the transform from last time
		transformPointCloud(*nextCloud, *nextCloud, lastTransform);
		transformPointCloud(*toAlign, *toAlign, lastTransform);
		icp.setInputCloud(toAlign);
		icp.setInputTarget(alignModel);
		printf("Full Model #Points: %i\n", getNPoints(fullModel));
		printf("Next Cloud #Points: %i\n", getNPoints(nextCloud));
		printf("Downsampled #Points: %i\n", getNPoints(toAlign));
		kinectPCL Final;
		icp.align(Final);
		printf("has converged: %i, score: %g\n", icp.hasConverged(), icp.getFitnessScore());
		//Now snap the full version of the point clouds in place
		transformPointCloud(*nextCloud, *nextCloud, icp.getFinalTransformation());
		*fullModel += *nextCloud;
		if (nScans%10 == 0) {
			//Every tenth scan downsample the full model
			kinectPCL::Ptr nextFullModel(new kinectPCL);
			DownSample(fullModel, nextFullModel, FINE_LEAF_SIZE);
			*fullModel = *nextFullModel;
		}
		//Update the working transform
		lastTransform = lastTransform * icp.getFinalTransformation();

//////////////////////////////////////////////////////////////////////////////////
////ICP Align before I added the 50% rule		
		
bool ICPAlign(kinectPCL::Ptr& model, kinectPCL::Ptr& target, Eigen::Matrix4f& initialGuess, Eigen::Matrix4f* transform) {
	*transform = initialGuess;
	transformPointCloud(*model, *model, *transform);

	int Nmodel = getNPoints(model);
	int Ntarget = getNPoints(target);

	//Set up the KD tree used to search the target for nearest neightbors
	//and allocate space for the correspondence indices
	pcl::KdTreeFLANN<PointType> searchTree;
	searchTree.setInputCloud(target);
	int* modelCorresp = new int[Nmodel];
	int* lastModelCorresp = new int[Nmodel];
	int* targetCorresp = new int[Ntarget];
	float* correspDists = new float[Nmodel];
	vector<int> alignModelIndices(Nmodel);
	vector<int> alignTargetIndices(Ntarget);
	vector<int> NNIndex(1);
	vector<float> NNDist(1);
	kinectPCL::Ptr targetCorrespPoints(new kinectPCL);
	*targetCorrespPoints = *model;
	
	for (int i = 0; i < Nmodel; i++)
		lastModelCorresp[i] = -1;
	
	//ICP Iterations
	int iterations = 0;
	bool converged = false;
	double RMSD = FLT_MAX;
	while (!converged && iterations < MAX_ICP_ITERATIONS) {
		//Step 1: Find a set of 1-to-1 correspondences between the model and target
			
		//Reset correspondences
		for (int i = 0; i < Nmodel; i++) {
			modelCorresp[i] = -1;
			correspDists[i] = FLT_MAX;
		}
		for (int i = 0; i < Ntarget; i++) {
			targetCorresp[i] = -1;
		}
		kinectPCL::iterator iter = model->begin();
		int i = 0;
		while (iter != model->end()) {
			PointType P = *iter;
			if (searchTree.nearestKSearch(P, 1, NNIndex, NNDist) > 0) {
				int targetIndex = NNIndex[0];
				float dist = NNDist[0];
				//Index of the model point currently associated with this target point
				int currModelIndex = targetCorresp[targetIndex];
				bool closer = false;//Is this point closer
				if (currModelIndex == -1) {
					closer = true;
				}
				else if (dist < correspDists[currModelIndex]) {
					modelCorresp[currModelIndex] = -1;
					closer = true;
				}
				if (closer) {
					modelCorresp[i] = targetIndex;
					targetCorresp[targetIndex] = i;
					targetCorrespPoints->points[i] = target->points[targetIndex];
					correspDists[i] = dist;
				}
			}	
			iter++;
			i++;
		}
		int NCorresp = 0;
		for (i = 0; i < Nmodel; i++) {
			if (modelCorresp[i] != -1)
				NCorresp++;
		}
		if (NCorresp < MIN_ICP_CORRESPONDENCES) //Not enough correspondences
			break;
		alignModelIndices.resize(NCorresp);
		alignTargetIndices.resize(NCorresp);
		i = 0;
		for (int k = 0; k < Nmodel; k++) {
			if (modelCorresp[k] != -1) {
				alignModelIndices[i] = modelCorresp[k];
				alignTargetIndices[i] = k;
				i++;
			}
		}
		//cout << "Corresp : " << 100.0*(double)NCorresp/(double)Nmodel << "%\n";
		//Step 2: Find the optimal transformation between the points
		Eigen::Matrix4f nextTrans;
		//estimateRigidTransformationSVD(*model, alignModelIndices, *target, alignTargetIndices, nextTrans);
		estimateRigidTransformationSVD(*model, *targetCorrespPoints, nextTrans);
		transformPointCloud(*model, *model, nextTrans);
		debugPub->publish(model);//Publish the intermediate transformations for debugging
		(*transform) = (*transform)*nextTrans;
		
		//Step 3: Check for convergence
		//Either the correspondences haven't changed since last time...
		/*bool sameCorresps = true;
		for (int k = 0; k < Nmodel; k++) {
			if (modelCorresp[k] != lastModelCorresp[k]) {
				sameCorresps = false;
				break;
			}
		}
		if (sameCorresps) {
			converged = true;
			break;
		}
		//...or the point clouds are close enough to each other to quit
		RMSD = getRMSD(model, target, searchTree);
		if (RMSD < MAX_ICP_ERROR) {
			converged = true;
			break;
		}*/
		
		//Step 4: Save the correspondences from this time to check for convergence next time
		for (int k = 0; k < Nmodel; k++) {
			lastModelCorresp[k] = modelCorresp[k];
		}
		iterations++;
	}
	cout << "\nICP " << (converged?"converged":"didn't converge") << " after " << iterations << " iterations with RMSD " << RMSD  << "\n\n";
	//Free up correspondence arrays
	delete[] modelCorresp;
	delete[] lastModelCorresp;
	delete[] targetCorresp;
	delete[] correspDists;
	
	return converged;
}
